from mpudmp import MPU6050
from machine import I2C, UART, Pin, PWM, Timer, ADC

# Util function to remap one range into another
def remap(x, in_min, in_max, out_min, out_max):
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min


def readCmd(uart):
    if uart.any():
        command = uart.readline()
        if command:
            data = ''
            try:
                data = command.decode('ascii')
            except UnicodeError as e:
                data = ''
            data = data.strip()
            #print(data)
            if( len(data) == 20):
                step = 4
                result = [int(data[i:i+step], 16) for i in range(0, len(data), step)]
                
                vals = list(result)
                return vals
    return None


def initDMP():
    
    i2c = I2C(id=1, sda=Pin(14), scl=Pin(15), freq=2000_000)

    device_address = 0x68
    freq_divider = 0x04  # freq = 200Hz / (1 + value)  推荐 >=4

    # Make an MPU6050
    mpu = MPU6050(i2c, device_address, freq_divider)

    # Initiate your DMP
    mpu.dmp_initialize()
    mpu.set_DMP_enabled(True)
    
    return mpu
